#include "udf_global.h"

#if ( UDF_EN_03 > 0 && !defined(UDP_SUB_PROJECT) && !defined(UDT_SUB_PROJECT) && (defined(UDF03_SUB_PROJECT) || defined(UDF_PRJ)) )
#include "udsf.h"

#include "../UDF/rom_function2_process.c"
#include "../udsf/udsf_uart.c"
#include "../udsf/udsf_spi.c"
#include "../udsf/udsf_gpio.c"
#include "../udsf/udsf_delay.c"
//#include "../udsf/udsf_protocol.c"
#include "../udsf/udsf_lpw.c"
#include "../udsf/udsf_dbscan.c"
#include "../udsf/udsf_data_send.c"
#include "../udsf/udsf_udf_switch.c"
#include "../udsf/udsf_cstdlike.c"
#include "../udsf/udsf_data_acquisition.c"
#include "../udsf/udsf_cfar.c"
#include "../udsf/udsf_bbe.c"
#if 1
#include "../udsf/udsf_pointrst.c"

static char udsf_AsciiToDec(unsigned char * pAscii, unsigned char * pDec, int nLen)
{
	unsigned char Nibble[2] = {0};
	int i = 0;
	int j = 0;

	if (nLen%2)		//2的非整数倍
	{
		Nibble[0] = *pAscii ++;
		if (Nibble[0] >= '0' && Nibble[0] <= '9')
		{
			pDec[0] = Nibble[0] - '0';
		}
		else
			return 1;//Nibble[j] = Nibble[j] - 'a' + 10;
		for (i = 0; i < (nLen-1)/2; i ++)
		{
			Nibble[0] = *pAscii ++;
			Nibble[1] = *pAscii ++;
			for (j = 0; j < 2; j ++)
			{
				if (Nibble[j] >= '0' && Nibble[j] <= '9')
					Nibble [j] = Nibble[j] - '0';
				else
					return 1;//Nibble[j] = Nibble[j] - 'a' + 10;
			}	// for (int j = ...)
			pDec[i+1] = Nibble[0] *10;// Set the high nibble
			pDec[i+1] += Nibble[1];	  // Set the low nibble
		}	// for (int i = ...)
	}
	else	//2的整数倍
	{
		for (i = 0; i < nLen/2; i ++)
		{
			Nibble[0] = *pAscii ++;
			Nibble[1] = *pAscii ++;
			for (j = 0; j < 2; j ++)
			{
				if (Nibble[j] >= '0' && Nibble[j] <= '9')
					Nibble [j] = Nibble[j] - '0';
				else
					return 1;//Nibble[j] = Nibble[j] - 'a' + 10;
			}	// for (int j = ...)
			pDec[i] = Nibble[0] *10;// Set the high nibble
			pDec[i] += Nibble[1];	// Set the low nibble
		}	// for (int i = ...)
	}
	return 0;
}
static void udsf_uart_recv_process(void){
			uint8_t frame_data[50];
			uint8_t AtoH[10]={0};
			int len = udsf_uart_recv_frame_data(frame_data,sizeof(frame_data));
			if(len>0){
//				printf ("\r\nlen=%d",len);
			len	--;			
			if(memcmp(frame_data,"AT+TriAngle=",12)==0&&(len-12>0 &&len-12<5)){
			uint32_t temp = 0;
			uint8_t string_len = 12;
			uint32_t tempParam[3] = {0};
			if(udsf_AsciiToDec(frame_data+string_len,AtoH,len-string_len)==0)
				{
					if((len-string_len)==1||(len-string_len)==2) temp = AtoH[0];		//1000-9999
					if((len-string_len)==3||(len-string_len)==4) temp = AtoH[0]*100+AtoH[1]; 
					if(temp>0&&temp<7){							
						udsf_spiflash_read_data((uint8_t *)&tempParam[0], FLASH_TRIDISLEVEL_ADDR, 12);
						tempParam[1]= temp;
						udsf_spiflash_write_data((uint8_t *)&tempParam[0],FLASH_TRIDISLEVEL_ADDR,12);
						udsf_spiflash_read_data((uint8_t *)&paramRSV->triAngle, FLASH_OUTDELAY_ADDR, 4);
//							printf ("temp=%dcm\n", paramRSV->triDisLevel);
					}
				}
				printf ("TriAngle=%d\n", paramRSV->triAngle);
			}
			
			else	if(memcmp(frame_data,"AT+INQUIRE",10)==0&&(len==10))  //查询参数
					printf("Tri_Dis=%d,TriAngle=%d\n",paramRSV->triDisLevel, paramRSV->triAngle);
			else if(memcmp(frame_data,"AT+TriDis=",10)==0&&(len-10>0 &&len-10<5)){       
					uint32_t temp = 0;
					uint8_t string_len = 10;
					uint32_t tempParam[3] = {0};
					if(udsf_AsciiToDec(frame_data+string_len,AtoH,len-string_len)==0)
					{
						if((len-string_len)==1||(len-string_len)==2) temp = AtoH[0];		//1000-9999
						if((len-string_len)==3||(len-string_len)==4) temp = AtoH[0]*100+AtoH[1]; 
						if(temp>0&&temp<6){							
							udsf_spiflash_read_data((uint8_t *)&tempParam[0], FLASH_TRIDISLEVEL_ADDR, 12);
							tempParam[0]= temp;
							udsf_spiflash_write_data((uint8_t *)&tempParam[0],FLASH_TRIDISLEVEL_ADDR,12);
							udsf_spiflash_read_data((uint8_t *)&paramRSV->triDisLevel, FLASH_TRIDISLEVEL_ADDR, 4);
//							printf ("temp=%dcm\n", paramRSV->triDisLevel);
						}
					}
					printf ("TriDis=%d\n", paramRSV->triDisLevel);                 
				}				
			else if( memcmp(frame_data,"AT",2)==0&& len==2 ){
				// 收到AT复位
				WDT_RSTNOW();
				while(1);
			}
			udsf_delay_ms(2000);
				}	
	}
static void udsf_find_maxpowIDX(uint32_t src_addr,uint8_t Vth,uint8_t rangeMin, uint8_t rangeMax, STRUCT_POINTRST *maxP,uint32_t Power_th)
{
	uint32_t everyPow = 0;
	
	maxP->target[0].d1Idx = 0;
	maxP->target[0].d2Idx = 0;
	maxP->target[0].d3Idx = 0;
	maxP->target[0].powABS= 0;
	maxP->targetNum = 0;
	
	for(uint32_t i = rangeMin; i < rangeMax; i++)
	{
		for(uint32_t j = 0; j < Vth; j++)
		{
			everyPow = Read_M32(src_addr + BBE_ABS_OFFSET + j*4 + i*UDFDEF_CHIRP_NUM*4);
			if(everyPow > maxP->target[0].powABS )
			{
				maxP->target[0].powABS = everyPow;
				
				//单位全部转成cm，用整型表示
				maxP->target[0].d1Idx = i;
				maxP->target[0].d2Idx = j;// 此处必须为正数，否则后续计算角度会出错
			}
		}
		for(uint32_t j = 32-Vth; j < UDFDEF_CHIRP_NUM; j++)
		{
			everyPow = Read_M32(src_addr + BBE_ABS_OFFSET + j*4 + i*UDFDEF_CHIRP_NUM*4);
			if(everyPow > maxP->target[0].powABS )
			{
				maxP->target[0].powABS = everyPow;
				
				//单位全部转成cm，用整型表示
				maxP->target[0].d1Idx = i;
				maxP->target[0].d2Idx = j;// 此处必须为正数，否则后续计算角度会出错
			}
		}
	}

//	printf("maxPow=%d,maxPowR=%d,maxPowV=%d\r\n", maxP->maxPow, maxP->maxPowR, maxP->maxPowV); 

	maxP->target[0].powABS = maxP->target[0].powABS / UDFDEF_CHIRP_SUM_NUM;// 适当缩小，以防输出16位溢出
	if(maxP->target[0].powABS > Power_th)// /2  
	{
		maxP->targetNum = 1;
	}
	else 
	{
	 maxP->targetNum = 0;
	}
}

static void udsf_read_param_from_flash(PARAM_RSV * params)
{
	uint32_t tempParam[3] = {0};
	uint8_t wrFlag = 0;
	
	udsf_spiflash_read_data((uint8_t *)&tempParam[0], FLASH_TRIDISLEVEL_ADDR, 4);
	udsf_spiflash_read_data((uint8_t *)&tempParam[1], FLASH_OUTDELAY_ADDR, 4);
	udsf_spiflash_read_data((uint8_t *)&tempParam[2], FLASH_PTHRES_ADDR, 4);

	if(!(tempParam[0] > 0 && tempParam[0] <=5))
	{
		tempParam[0] = TRIDISLEVEL_DEFAULT;
		wrFlag = 1;
	}
	else
	{
		params->triDisLevel = tempParam[0];
	}

	if(!(tempParam[1] > 0 && tempParam[1] <7))
	{
		tempParam[1] = OUTDELAY_DEFAULT;
		wrFlag = 1;
	}
	else
	{
		params->triAngle = tempParam[1];
	}

	if(!(tempParam[2] > 1000 && tempParam[2] < 20000))
	{
		tempParam[2] = POWERTHRES_DEFAULT;
		wrFlag = 1;
	}
	else
	{
		params->powThres = tempParam[2];
	}
//	params->powThres = POWERTHRES_DEFAULT;
	
	if(wrFlag == 1)
	{
		udsf_spiflash_erase_sector(FLASH_TRIDISLEVEL_ADDR);
		udsf_spiflash_write_data((uint8_t *)&tempParam[0], FLASH_TRIDISLEVEL_ADDR, 12);
		udsf_spiflash_read_data((uint8_t *)&params->triDisLevel, FLASH_TRIDISLEVEL_ADDR, 4);
		udsf_spiflash_read_data((uint8_t *)&params->triAngle, FLASH_OUTDELAY_ADDR, 4);
		udsf_spiflash_read_data((uint8_t *)&params->powThres, FLASH_PTHRES_ADDR, 4);
	}
}

void UDF03_udf_handle(void){
	if( paramDataPrep->switch_flag == 0 ){
		#if (UDK_MODE_SETS == UDK_VERSION_LITE) || (DYNALOAD_FW == 1)
		udsf_udkv2_init(); // 为兼容ROM启动，主UDF中必须首先调用此函数。UDK-SE版本无需兼容性考虑;UDK-STD版本USRBOOT内已调用此函数,无需重复调用。
		#endif
		UDK_ASSERT((p_gb_sysboot->boot_flag == APPLICATION_MODE)); // 确认程序应用模式配置正常
		#if DYNALOAD_FW == 1
		udsf_uart_init(paramFunc->baudrate,p_gb_sysboot->boot_flag);
		#define DELAY_MS (20)  // RC2412 UART-DL Tools 以启动20ms后接收到AT+OK\n来判断动态加载是否成功。动态加载用户在最终应用中可去除该部分
		#if DELAY_MS > 0
		if( p_gb_sysboot->boot_flag == 0 ){
			udsf_delay_ms_rom(DELAY_MS);
		}else{
			udsf_delay_ms_xip(DELAY_MS);
		}
		udsf_uart_send_string("AT+OK\n");
		#endif
		#elif UDK_MODE_SETS == UDK_VERSION_STD
		udsf_uart_init(paramFunc->baudrate,p_gb_sysboot->boot_flag);
		#endif

		#if (DYNALOAD_FW == 1)
		GPIO->PMODE  = paramFunc->gpio_pmode;
		PIN_CTRL->UE = paramFunc->gpio_pctrl_ue;
		PIN_CTRL->RE = paramFunc->gpio_pctrl_re;
		PINMUX->MUX0 = paramFunc->gpio_pmux0;
		PINMUX->MUX1 = paramFunc->gpio_pmux1;
		#endif

		RTC0->IRQ_CFG |= RTC_IRQ_MASK_Pos;   // rtc0 irq disable
		RTC0->IRQ_CFG |= RTC_IRQ_CLEAR_Msk;  // rtc0 irq flag clear
		SYSC->CLK_EN  |= RTC0_PATTERN_SW_Msk;// rtc0 clk disable

		RTC1->IRQ_CFG |= RTC_IRQ_MASK_Pos;    // rtc1 irq disable
		RTC1->IRQ_CFG |= RTC_IRQ_CLEAR_Msk;   // rtc1 irq flag clear
		SYSC->CLK_EN  |= RTC1_PATTERN_SW_Msk; // rtc1 clk disable
	}
	#if (DEBUG_DATA_INTERFACE == 1) || ( POINTRST_INTERFACE == 1) || (LOG_INTERFACE == 1) || ( CUSTOM_DATA_INTERFACE == 1 )
	udsf_spi_init();
	#endif






	#ifdef UART_RECV_ENABLE
//	uint8_t frame_data[50];
	/* disable uart irq */
	NVIC->ICER[0] = (1 << ((uint32_t)(USCI1_IRQn) & 0x1F));
	USCI1->IMR=0xFFFFFFFF;	
	
	/* set workmode->fake config mode */
	*p_work_mode = 0;

	/* enable uart irq */
	USCI1->ISR = 0x1F;
	USCI1->IMR &= ~USCI_IMR_RXNE_Msk;
	NVIC->ISER[0] = (1 << ((uint32_t)(USCI1_IRQn) & 0x1F));
	#endif
	
	if( paramRSV->startup_flag ){

		paramRSV->startup_flag = 0;

		sUDF_global.pcrst_freq_cnt=0;
		sUDF_global.frame_cnt = 0;
		udsf_read_param_from_flash(paramRSV);
			
		udsf_clear_track_data(&sUDF_global.track);
		
		memset((uint8_t *)(UDFDEF_ADDR_POINT_RST + 84), 0, 120); //必须清
	}

	while(1){	

		#if ( SWITCH_INNER_UDF==1 )
		if( paramDataPrep->switch_flag == 0 )
		#endif
		{
			
			WDT->STR = 1;
			

			uint8_t trigerDistance[5] = {200, 180, 150, 120, 100};
			udsf_rtc_set(0,paramRSV->frame_interval*34);
			
			#ifdef UART_RECV_ENABLE
//			int len = udsf_uart_recv_frame_data(frame_data,sizeof(frame_data));
//					if(udsf_strstr((char*)frame_data,"OTA START")&&1){  //udsf_strstr((char*)recv_data,"OTA START")
//					udsf_udf_switch_without_crc(3,9);
//					break;
//			}
				
				udsf_uart_recv_process();
//			else if(udsf_strstr((char*)frame_data, "OTA START") && 1)
//			{
//				//udsf_strstr((char*)recv_data, "OTA START")
//				udsf_udf_switch_without_crc(3, 9);
//				break;
//				}
			#endif
			
			#ifdef FLASH_WINRANGE
			#if UDFDEF_ADCSAMP_NUM == 64
			udsf_memory_load2sram(APP_RSV_ADDR+256*2,p_gb_sysboot->boot_flag,p_gb_sysboot->udt_src,(uint8_t*)UDFDEF_ADDR_WIN_RANGE,UDFDEF_ADCSAMP_NUM*2);
			#elif UDFDEF_ADCSAMP_NUM == 256
			udsf_memory_load2sram(APP_RSV_ADDR+64*2,p_gb_sysboot->boot_flag,p_gb_sysboot->udt_src,(uint8_t*)UDFDEF_ADDR_WIN_RANGE,UDFDEF_ADCSAMP_NUM*2);
			#else
			#error unsupport@rsv.c
			#endif


			
			#if ( UDFDEF_CHIRP_SUM_NUM == 8 ) || ( UDFDEF_CHIRP_SUM_NUM == 3 )
			udsf_data_acquisiton_chirpsum(1,1,0,0);
			#elif ( UDFDEF_CHIRP_SUM_NUM == 1 )
			// chirp间隔误差最大30us
			#if (RAMP_BANDWIDTH == 0)
			udsf_data_acquisiton_chirpsum(1,1,0,(496-270)); // 固定减270us，chirp间隔：wave0(A+B+C)+wave1(A+B+C) + x --> 10+1+10+10+17+30 -20 + x = 567
			#elif (RAMP_BANDWIDTH == 1)
			udsf_data_acquisiton_chirpsum(1,1,1,(450-270)); // 固定减270us，chirp间隔：wave0(A+B+C)+wave1(A+B+C) + x -->  10+1+10+10+60+30 -20 + x = 551
			#else
			#error unknown bandwidth
			#endif
			#else
			#error unmatched
			#endif
			#endif

			int ant_id = 1;
			do{
				uint32_t src_addr = UDFDEF_ADDR_FFT1D_ANT1;
				if( ant_id == 2 ){
					src_addr = UDFDEF_ADDR_FFT1D_ANT2;
				}
				
				paramALG->alg25.p2Cfg4 = (paramALG->alg25.p2Cfg4&0xffff0000) | (GET_OFFSET(src_addr))<< 0;
				paramALG->alg25.p2Cfg8 = (paramALG->alg25.p2Cfg8&0xffff0000) | (GET_OFFSET(src_addr))<< 0;

				P2_FFT_HANDLE(&paramALG->alg25);
				

				paramALG->alg1.p2Cfg4 = (paramALG->alg1.p2Cfg4&0xffff0000) | (GET_OFFSET(src_addr))<< 0,
				P2_FFT_HANDLE(&paramALG->alg1); // ant1 mean step1
				

				
				#if defined(PRESENCE_ENABLE)
				if( ant_id == 1 ){
					// 均值fifo-> UDFDEF_ADDR_PRESENCE_FFT1D_ANT1
					for( int i=0;i<UDFDEF_RANGE_USE;i++){
						
						for( int j=0;j<UDFDEF_CHIRP_NUM-1;j++){
//							udsf_memcpy32(((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT1)+j+1+i*UDFDEF_CHIRP_NUM,((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT1)+
//							j+i*UDFDEF_CHIRP_NUM,1);
							
							((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT1)[j+i*UDFDEF_CHIRP_NUM] = ((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT1)[j+1+i*UDFDEF_CHIRP_NUM];
						}
						
//						udsf_memcpy32(((uint32_t*)UDFDEF_ADDR_FFT1D_MEAN)+i,((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT1)+
//						(UDFDEF_CHIRP_NUM-1)+i*UDFDEF_CHIRP_NUM,1)
						((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT1)[(UDFDEF_CHIRP_NUM-1)+i*UDFDEF_CHIRP_NUM] = ((uint32_t*)UDFDEF_ADDR_FFT1D_MEAN)[i];
						
						//((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT1)[(UDFDEF_CHIRP_NUM-1)+i*UDFDEF_CHIRP_NUM] = ((uint32_t*)UDFDEF_ADDR_FFT1D_ANT1)[(UDFDEF_CHIRP_NUM-1)+i*UDFDEF_CHIRP_NUM];
						
//						udsf_memcpy32(((uint32_t*)UDFDEF_ADDR_FFT1D_MEAN)+i,((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT1)+
//						(sUDF_global.chirp_fifo_index%UDFDEF_CHIRP_NUM)+i*UDFDEF_CHIRP_NUM,1);
					}
				
				}else{
					// 均值fifo-> UDFDEF_ADDR_PRESENCE_FFT1D_ANT2
					for( int i=0;i<UDFDEF_RANGE_USE;i++){
						
						for( int j=0;j<UDFDEF_CHIRP_NUM-1;j++){
//							udsf_memcpy32(((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT2)+j+1+i*UDFDEF_CHIRP_NUM,((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT2)+
//							j+i*UDFDEF_CHIRP_NUM,1);
							((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT2)[j+i*UDFDEF_CHIRP_NUM] = ((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT2)[j+1+i*UDFDEF_CHIRP_NUM];
						}
						
//						udsf_memcpy32(((uint32_t*)UDFDEF_ADDR_FFT1D_MEAN)+i,((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT2)+
//						(UDFDEF_CHIRP_NUM-1)+i*UDFDEF_CHIRP_NUM,1);
						((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT2)[(UDFDEF_CHIRP_NUM-1)+i*UDFDEF_CHIRP_NUM] = ((uint32_t*)UDFDEF_ADDR_FFT1D_MEAN)[i];
						

//						udsf_memcpy32(((uint32_t*)UDFDEF_ADDR_FFT1D_MEAN)+i,((uint32_t*)UDFDEF_ADDR_PRESENCE_FFT1D_ANT2)+
//						(sUDF_global.chirp_fifo_index%UDFDEF_CHIRP_NUM)+i*UDFDEF_CHIRP_NUM,1);
					}
//					sUDF_global.chirp_fifo_index = (sUDF_global.chirp_fifo_index+1)%UDFDEF_CHIRP_NUM;
				}
				#endif

				if( ant_id == 1 ){
					{
						paramALG->alg5.p2Cfg4 = (paramALG->alg5.p2Cfg4&0xffFF0000) | (GET_OFFSET(src_addr))<<0; 

						P2_FFT_HANDLE(&paramALG->alg5);

						STRUCT_POINTRST *frameRst = (STRUCT_POINTRST *)(UDFDEF_ADDR_POINT_RST);
						
						udsf_find_maxpowIDX(UDFDEF_ADDR_FFT2D_ANT1,16,0,18,frameRst,10000);
//						if(!frameRst->targetNum)
//							udsf_find_maxpowIDX(UDFDEF_ADDR_FFT2D_ANT1,16,9,18,frameRst,20000);
//						uint8_t frame_head[8] = {1, 2, 3, 4, 5, 6, 7, 8};
//						udsf_uart_send_data(frame_head, 8);
//						udsf_uart_send_data((uint8_t*)UDFDEF_ADDR_FFT2D_ANT1, 40*4*DEMO_CHIRP_NUM);
						
						STRUCT_POINTRST maxpower_far={0};
						udsf_find_maxpowIDX(UDFDEF_ADDR_FFT2D_ANT1,0,18,25,&maxpower_far,10000);
//						if(!frameRst->targetNum){
//							frameRst->target[cnt].d3Idx
//						}
						
						if(frameRst->targetNum > 0 && maxpower_far.targetNum > 0
					&& maxpower_far.target[0].powABS >= 3*frameRst->target[0].powABS)
				{
						frameRst->targetNum = 0;
				}	
//if( frameRst->targetNum)						
//printf("\r\nDel: R = %d, V = %d, P = %d", frameRst->target[0].d1Idx,frameRst->target[0].d2Idx, frameRst->target[0].powABS);						
						if( frameRst->targetNum > 0 && (UDFDEF_ANT_NUM > 1)){
							FFT3D_PRE_HANDLE(&paramALG->alg12, frameRst);
						}
					}
					#if defined(PRESENCE_ENABLE)
					STRUCT_POINTRST *frameRst_presence = (STRUCT_POINTRST *)(UDFDEF_ADDR_POINT_RST_PRESENCE);
					
					if( sUDF_global.frame_cnt >= (UDFDEF_CHIRP_NUM-1) ){ /* 存在检测@ant1 */						
						src_addr = UDFDEF_ADDR_PRESENCE_FFT1D_ANT1;
						
						paramALG->alg1.p2Cfg4 = (paramALG->alg1.p2Cfg4&0xffff0000) | (GET_OFFSET(src_addr))<< 0;
						P2_FFT_HANDLE(&paramALG->alg1); // ant1 mean step1

						paramALG->alg5.p2Cfg4 = (paramALG->alg5.p2Cfg4&0xffFF0000) | (GET_OFFSET(src_addr))<<0; 

						P2_FFT_HANDLE(&paramALG->alg5);

						udsf_find_maxpowIDX(UDFDEF_ADDR_FFT2D_ANT1,16,0,16,frameRst_presence,5000);
//						if( !frameRst_presence->targetNum)	
//							udsf_find_maxpowIDX(UDFDEF_ADDR_FFT2D_ANT1,16,10,13,frameRst_presence,10000);
//if( frameRst_presence->targetNum)						
//	printf("\r\nDel: R = %d, V = %d, P = %d", frameRst_presence->target[0].d1Idx,frameRst_presence->target[0].d2Idx, frameRst_presence->target[0].powABS);
	
//						uint8_t frame_head[8] = {1, 2, 3, 4, 5, 6, 7, 8};
//						udsf_uart_send_data(frame_head, 8);
//						udsf_uart_send_data((uint8_t*)UDFDEF_ADDR_FFT2D_ANT1, 40*4*DEMO_CHIRP_NUM);
											
						if( frameRst_presence->targetNum > 0 && (UDFDEF_ANT_NUM > 1)){
							uint32_t tmp = paramALG->alg12.dstAddr;
							paramALG->alg12.dstAddr = UDFDEF_ADDR_FFT3D_PRE_PRESENCE + NUM_BYTE32;
							FFT3D_PRE_HANDLE(&paramALG->alg12, frameRst_presence);
							paramALG->alg12.dstAddr = tmp;
						}

					}else{
						frameRst_presence->targetNum = 0;
					}
					#endif
				}else{
					{
						STRUCT_POINTRST *frameRst = (STRUCT_POINTRST *)(UDFDEF_ADDR_POINT_RST);
						if( frameRst->targetNum > 0 ){

							paramALG->alg6.p2Cfg4 = (paramALG->alg6.p2Cfg4&0xffFF0000) | (GET_OFFSET(src_addr))<<0; 
							P2_FFT_HANDLE(&paramALG->alg6);

							//FFT3D Pre Ant2
							FFT3D_PRE_HANDLE(&paramALG->alg20, frameRst);
							udsf_memcpy((uint8_t*)UDFDEF_ADDR_ARRAY_CALIB,(uint8_t*)UDFDEF_ADDR_ARRAY_CALIB_RUN,NUM_ANGLE*NUM_BYTE32*ANT_MAX_NUM);

							// fft3d dft
							bbe_regb_str->P2_CFG0 = paramALG->alg23.p2Cfg0;
							bbe_regb_str->P2_CFG1 = paramALG->alg23.p2Cfg1;
							bbe_regb_str->P2_CFG2 = paramALG->alg23.p2Cfg2;
							bbe_regb_str->P2_CFG3 = paramALG->alg23.p2Cfg3;
							bbe_regb_str->P2_CFG4 = paramALG->alg23.p2Cfg4;
							bbe_regb_str->P2_CFG5 = paramALG->alg23.p2Cfg5;
							//bbe_regb_str->P2_CFG6 = paramALG->alg23.p2Cfg6;
							bbe_regb_str->P2_CFG7 = paramALG->alg23.p2Cfg7;
							bbe_regb_str->P2_CFG8 = paramALG->alg23.p2Cfg8;
							BBE_ISR_CLEAR(BBE_ISR_P2_JUMP_END);
							BBE_OPGATE_EN(BBE_GATE_P2);

							for (uint32_t cnt = 0; cnt < frameRst->targetNum; cnt++)
							{
									bbe_regb_str->P2_CFG6 = paramALG->alg23.p2Cfg6 + cnt * ANT_MAX_NUM * NUM_BYTE32;
									BBE_OPTRIG(BBE_TRIG_P2);
									BBE_WAITISR(BBE_ISR_P2_JUMP_END);
									frameRst->target[cnt].d3Idx = BBE_P2_STA0_MAXJUMPIDX;
						
									if(frameRst->target[cnt].d3Idx >= NUM_ANGLE/2)
										frameRst->target[cnt].d3Idx = frameRst->target[cnt].d3Idx - NUM_ANGLE;
									else
										frameRst->target[cnt].d3Idx = frameRst->target[cnt].d3Idx;

									if(frameRst->target[cnt].d2Idx >= UDFDEF_CHIRP_NUM/2)
									{
										frameRst->target[cnt].d2Idx = frameRst->target[cnt].d2Idx - UDFDEF_CHIRP_NUM;
									}
//									printf("\r\n[%d]: R = %d, V = %d, A= %d, P = %d", cnt,frameRst->target[cnt].d1Idx,frameRst->target[cnt].d2Idx,frameRst->target[cnt].d3Idx, frameRst->target[cnt].powABS);							
							}
//if( frameRst->targetNum)						
//printf("\r\nDel: R = %d, V = %d, A= %d, P = %d", frameRst->target[0].d1Idx,frameRst->target[0].d2Idx,frameRst->target[0].d3Idx, frameRst->target[0].powABS);							
//else
//	printf
						}
					}

					#if defined(PRESENCE_ENABLE)
					if( sUDF_global.frame_cnt >= (UDFDEF_CHIRP_NUM-1) ){ /*存在检测@ant2*/
						STRUCT_POINTRST *frameRst_presence = (STRUCT_POINTRST *)(UDFDEF_ADDR_POINT_RST_PRESENCE);

						if( frameRst_presence->targetNum > 0 || (TEST_MODE == 7)){
							src_addr = UDFDEF_ADDR_PRESENCE_FFT1D_ANT2;
							paramALG->alg1.p2Cfg4 = (paramALG->alg1.p2Cfg4&0xffff0000) | (GET_OFFSET(src_addr))<< 0,
							P2_FFT_HANDLE(&paramALG->alg1); // ant1 mean step1
							paramALG->alg6.p2Cfg4 = (paramALG->alg6.p2Cfg4&0xffFF0000) | (GET_OFFSET(src_addr))<<0; 
							P2_FFT_HANDLE(&paramALG->alg6);

							//FFT3D Pre Ant2
							uint32_t tmp = paramALG->alg20.dstAddr;
							paramALG->alg20.dstAddr = UDFDEF_ADDR_FFT3D_PRE_PRESENCE;
							FFT3D_PRE_HANDLE(&paramALG->alg20, frameRst_presence);
							paramALG->alg20.dstAddr = tmp;

							udsf_memcpy((uint8_t*)UDFDEF_ADDR_ARRAY_CALIB,(uint8_t*)UDFDEF_ADDR_ARRAY_CALIB_RUN,NUM_ANGLE*NUM_BYTE32*ANT_MAX_NUM);
							// fft3d dft
							bbe_regb_str->P2_CFG0 = paramALG->alg23.p2Cfg0;
							bbe_regb_str->P2_CFG1 = paramALG->alg23.p2Cfg1;
							bbe_regb_str->P2_CFG2 = paramALG->alg23.p2Cfg2;
							bbe_regb_str->P2_CFG3 = paramALG->alg23.p2Cfg3;
							bbe_regb_str->P2_CFG4 = paramALG->alg23.p2Cfg4;
							bbe_regb_str->P2_CFG5 = paramALG->alg23.p2Cfg5;
							//bbe_regb_str->P2_CFG6 = paramALG->alg23.p2Cfg6;
							bbe_regb_str->P2_CFG7 = paramALG->alg23.p2Cfg7;
							bbe_regb_str->P2_CFG8 = paramALG->alg23.p2Cfg8;
							BBE_ISR_CLEAR(BBE_ISR_P2_JUMP_END);
							BBE_OPGATE_EN(BBE_GATE_P2);
							uint32_t cfg6 = (NUM_BYTE32) << 16 | (GET_OFFSET(UDFDEF_ADDR_FFT3D_PRE_PRESENCE)) << 0;
							for (uint32_t cnt = 0; cnt < frameRst_presence->targetNum; cnt++)
							{
								bbe_regb_str->P2_CFG6 = cfg6 + cnt * ANT_MAX_NUM * NUM_BYTE32;
								BBE_OPTRIG(BBE_TRIG_P2);
								BBE_WAITISR(BBE_ISR_P2_JUMP_END);
								frameRst_presence->target[cnt].d3Idx = BBE_P2_STA0_MAXJUMPIDX;
								//LOG_SEND_U8_BASE16(LOG_LEVEL_EMERG,&frameRst_presence->target[cnt].d3Idx,1,"d3:","\n");
								if(frameRst_presence->target[cnt].d3Idx >= NUM_ANGLE/2)
										frameRst_presence->target[cnt].d3Idx = frameRst_presence->target[cnt].d3Idx - NUM_ANGLE;
									else
										frameRst_presence->target[cnt].d3Idx = frameRst_presence->target[cnt].d3Idx;

									if(frameRst_presence->target[cnt].d2Idx >= UDFDEF_CHIRP_NUM/2)
									{
										frameRst_presence->target[cnt].d2Idx = frameRst_presence->target[cnt].d2Idx - UDFDEF_CHIRP_NUM;
									}					
//					printf("\r\nr=%d, v=%d, a=%d, p=%d", frameRst_presence->target[cnt].d1Idx, frameRst_presence->target[cnt].d2Idx,frameRst_presence->target[cnt].d3Idx,frameRst_presence->target[cnt].powABS);
							}							
						}
					}
					#endif
				}

				if( ++ant_id > UDFDEF_ANT_NUM ){
					break;
				}
			}while(1);		

			if( ((sUDF_global.frame_cnt+1) % POINTRST_USER_PROCESS_PER_N_FRAME) == 0 ){
				#if ( SWITCH_INNER_UDF == 1 )
				udsf_udf_switch_without_crc(3,1); //  较udsf_udf_switch(3,1,0)节省200字节左右code
				break;
				#endif
			}

		}	
	
		#if ( SWITCH_INNER_UDF==1 )
		paramDataPrep->switch_flag = 0;
		#endif
		
//		printf ("it is ok");
		sUDF_global.frame_cnt++;
		
		if( sUDF_global.frame_cnt == UINT32_MAX ){
			// demo流程未做溢出处理，整机复位
			WDT_RSTNOW();
			while(1);
		}

		udsf_rtc_get_block(0); // wait rtc0

	}
}
#else

#endif
#endif
